
#include "ImageEvaluate.h"
#include "ImgQuaEv.h"

#define BITS_16 1
#define BITS_8 0
struct QImgType
{
	static const int mImgBits = BITS_8;
};

#define FOCAL_IMGNUM 7  //5.0 step


///////////////////////////////////
bool testInts(int a[])
{
	if (a[1] > a[3])
	{
		if (a[0] > a[3] || a[4] > a[0]) return false;
	}
	else
	{
		if (a[1] < a[4] || a[4] < a[0]) return false;
	}
	return true;
}

bool basicPeak(const int x, const int y, cv::Mat &img, const int threshold)
{
	if (QImgType::mImgBits == BITS_16)
	{
		int center = img.at<unsigned short>(x, y);
		int pixel3_3[9] = { img.at<unsigned short>(x - 1, y - 1),
			img.at<unsigned short>(x - 1, y),
			img.at<unsigned short>(x - 1, y + 1),
			img.at<unsigned short>(x, y - 1),
			center,
			img.at<unsigned short>(x, y + 1),
			img.at<unsigned short>(x + 1, y),
			img.at<unsigned short>(x + 1, y - 1),
			img.at<unsigned short>(x + 1, y + 1)
		};
		for (int i = 0; i < 4; i++)
		{
			if (center < pixel3_3[i]) return false;
			if (threshold > pixel3_3[i]) return false;
		}
		for (int i = 5; i < 9; i++)
		{
			if (center < pixel3_3[i]) return false;
			if (threshold > pixel3_3[i]) return false;
		}
		return true;
	}
	else
	{
		int center = img.at<unsigned char>(x, y);
		int pixel3_3[9] = { img.at<unsigned char>(x - 1, y - 1),
			img.at<unsigned char>(x - 1, y),
			img.at<unsigned char>(x - 1, y + 1),
			img.at<unsigned char>(x, y - 1),
			center,
			img.at<unsigned char>(x, y + 1),
			img.at<unsigned char>(x + 1, y),
			img.at<unsigned char>(x + 1, y - 1),
			img.at<unsigned char>(x + 1, y + 1)
		};
		for (int i = 0; i < 4; i++)
		{
			if (center < pixel3_3[i]) return false;
			if (threshold > pixel3_3[i]) return false;
		}
		for (int i = 5; i < 9; i++)
		{
			if (center < pixel3_3[i]) return false;
			if (threshold > pixel3_3[i]) return false;
		}
		return true;
	}	
}

bool goodPeakJudge(int x, int y, cv::Mat &img, const int threshold)
{
	if (!basicPeak(x, y, img, threshold))
		return false;
	int count = 0;
	//unsigned short cmp[5] = { 0 };
	if (QImgType::mImgBits == BITS_16)
	{
		unsigned short centerVal = img.at<unsigned short>(x, y);

		if (img.at<unsigned short>(x - 1, y - 1)< img.at<unsigned short>(x - 2, y - 2)) count++;
		if (img.at<unsigned short>(x + 1, y + 1) < img.at<unsigned short>(x + 2, y + 2)) count++;
		if (img.at<unsigned short>(x + 1, y - 1) < img.at<unsigned short>(x + 2, y - 2)) count++;
		if (img.at<unsigned short>(x - 1, y + 1) < img.at<unsigned short>(x - 2, y + 2)) count++;

		if (img.at<unsigned short>(x - 1, y) < img.at<unsigned short>(x - 2, y)) count++;
		if (img.at<unsigned short>(x + 1, y)< img.at<unsigned short>(x + 2, y)) count++;
		if (img.at<unsigned short>(x, y - 1) < img.at<unsigned short>(x, y - 2)) count++;
		if (img.at<unsigned short>(x, y + 1) < img.at<unsigned short>(x, y + 2)) count++;

		int a[5] = { 0 };
		a[0] = img.at<unsigned short>(x - 2, y);
		a[1] = img.at<unsigned short>(x - 1, y);
		a[2] = centerVal;
		a[3] = img.at<unsigned short>(x + 1, y);
		a[4] = img.at<unsigned short>(x + 2, y);
		if (!testInts(a)) count++;
		a[0] = img.at<unsigned short>(x, y - 2);
		a[1] = img.at<unsigned short>(x, y - 1);
		a[3] = img.at<unsigned short>(x, y + 1);
		a[4] = img.at<unsigned short>(x, y + 2);
		if (!testInts(a)) count++;
	}
	else
	{
		unsigned char centerVal = img.at<unsigned char>(x, y);

		if (img.at<unsigned char>(x - 1, y - 1)< img.at<unsigned char>(x - 2, y - 2)) count++;
		if (img.at<unsigned char>(x + 1, y + 1) < img.at<unsigned char>(x + 2, y + 2)) count++;
		if (img.at<unsigned char>(x + 1, y - 1) < img.at<unsigned char>(x + 2, y - 2)) count++;
		if (img.at<unsigned char>(x - 1, y + 1) < img.at<unsigned char>(x - 2, y + 2)) count++;

		if (img.at<unsigned char>(x - 1, y) < img.at<unsigned char>(x - 2, y)) count++;
		if (img.at<unsigned char>(x + 1, y)< img.at<unsigned char>(x + 2, y)) count++;
		if (img.at<unsigned char>(x, y - 1) < img.at<unsigned char>(x, y - 2)) count++;
		if (img.at<unsigned char>(x, y + 1) < img.at<unsigned char>(x, y + 2)) count++;

		int a[5] = { 0 };
		a[0] = img.at<unsigned char>(x - 2, y);
		a[1] = img.at<unsigned char>(x - 1, y);
		a[2] = centerVal;
		a[3] = img.at<unsigned char>(x + 1, y);
		a[4] = img.at<unsigned char>(x + 2, y);
		if (!testInts(a)) count++;
		a[0] = img.at<unsigned char>(x, y - 2);
		a[1] = img.at<unsigned char>(x, y - 1);
		a[3] = img.at<unsigned char>(x, y + 1);
		a[4] = img.at<unsigned char>(x, y + 2);
		if (!testInts(a)) count++;		
	}
	if (count < 3)
		return true;
	else
		return false;
}

int getXmultiY(int x, int y, const cv::Mat &img, float threshold)
{
	int xLen = 1, yLen = 1;
	int p = 0;
	int c = img.cols;
	int r = img.rows;

	unsigned short CenterInts = img.at<unsigned char>(x, y);
	
	if (QImgType::mImgBits == BITS_16)
	{
		CenterInts = img.at<unsigned short>(x, y);
	}
	unsigned int sum = 0;

	while (x + xLen<c)
	{
		unsigned short Ints = img.at<unsigned char>(x + xLen, y);
	if (QImgType::mImgBits == BITS_16)
	{
		Ints = img.at<unsigned short>(x + xLen, y);
	}
		if (Ints < threshold)
			break;
		if (Ints / CenterInts > 1)
			sum++;
		if (sum > 3)
			return 0;
		xLen++;
	}

	p = 1; sum = 0;
	while (x - p>0)
	{
		unsigned short Ints = img.at<unsigned char>(x - p, y);
			if (QImgType::mImgBits == BITS_16)
	{
		Ints = img.at<unsigned short>(x - p, y);
	}
		if (Ints < threshold)
			break;
		if (Ints / CenterInts > 1)
			sum++;
		if (sum > 3)
			return 0;
		p++;
	}
	//int rateX = p>xLen ? p / xLen : xLen / p;
	if ((p>xLen ? p / xLen : xLen / p) > 2)
		return 0;
	xLen += p - 1;

	sum = 0;
	while (y + yLen<r)
	{
		unsigned short Ints = img.at<unsigned char>(x, y + yLen);
			if (QImgType::mImgBits == BITS_16)
	{
		Ints = img.at<unsigned short>(x, y + yLen);
	}		
		if (Ints < threshold)
			break;
		if (Ints / CenterInts > 1)
			sum++;
		if (sum > 3)
			return 0;
		yLen++;
	}
	p = 1; sum = 0;
	while (y - p>0)
	{
		unsigned short Ints = img.at<unsigned char>(x, y - p);
				if (QImgType::mImgBits == BITS_16)
	{
		Ints = img.at<unsigned short>(x, y - p);
	}	
		if (Ints < threshold)
			break;
		if (Ints / CenterInts > 1)
			sum++;
		if (sum > 3)
			return 0;
		p++;
	}
	if ((p>yLen ? p / yLen : yLen / p) > 2)
		return 0;
	yLen += p - 1;
	//std::cout << xLen*yLen << std::endl;
	return xLen*yLen;
}

float getScore(int x, int y, const cv::Mat &img, int s = 1)
{
	if (QImgType::mImgBits == BITS_16)
		{
        int EightPixelInts = img.at<unsigned short>(x - s, y) +
            img.at<unsigned short>(x - s, y - s) + img.at<unsigned short>(x - s, y + s) +
            img.at<unsigned short>(x + s, y) + img.at<unsigned short>(x + s, y - s) +
            img.at<unsigned short>(x + s, y + s) + img.at<unsigned short>(x, y - s) +
            img.at<unsigned short>(x, y + s);
	int centerPixelInts = img.at<unsigned short>(x, y);
	float meanPeakInts = 1.0*(EightPixelInts + centerPixelInts) / 9;
	//std::cout << meanPeakInts << " ";
	float score = 0;
	if(meanPeakInts != 0 )
	{
		score = 1.0*(8 * centerPixelInts - EightPixelInts) / meanPeakInts;
		}
	return score;
		}
	else
		{
        int EightPixelInts = img.at<unsigned char>(x - s, y) +
            img.at<unsigned char>(x - s, y - s) + img.at<unsigned char>(x - s, y + s) +
            img.at<unsigned char>(x + s, y) + img.at<unsigned char>(x + s, y - s) +
            img.at<unsigned char>(x + s, y + s) + img.at<unsigned char>(x, y - s) +
            img.at<unsigned char>(x, y + s);
	int centerPixelInts = img.at<unsigned char>(x, y);
	float meanPeakInts = 1.0*(EightPixelInts + centerPixelInts) / 9;
	float score = 0;
	if(meanPeakInts != 0 )
	{
		score = 1.0*(8 * centerPixelInts - EightPixelInts) / meanPeakInts;
	}

	return score;			
		}
}
float scoreFunc(int x, int y, const cv::Mat &img, float &X, float &Y)
{
	int v[9] = { 0 };
	if (QImgType::mImgBits == BITS_16)
		{
	v[0] = img.at<unsigned short>(x - 1, y - 1);
	v[1] = img.at<unsigned short>(x - 1, y);
	v[2] = img.at<unsigned short>(x - 1, y + 1); 
	v[3] = img.at<unsigned short>(x, y - 1);
	v[4] = img.at<unsigned short>(x, y);
	v[5] = img.at<unsigned short>(x, y + 1);
	v[6] = img.at<unsigned short>(x + 1, y - 1);
	v[7] = img.at<unsigned short>(x + 1, y);
	v[8] = img.at<unsigned short>(x + 1, y + 1);
		}
	else
		{
	v[0] = img.at<unsigned char>(x - 1, y - 1);
	v[1] = img.at<unsigned char>(x - 1, y);
	v[2] = img.at<unsigned char>(x - 1, y + 1);
	v[3] = img.at<unsigned char>(x, y - 1);
	v[4] = img.at<unsigned char>(x, y);
	v[5] = img.at<unsigned char>(x, y + 1);
	v[6] = img.at<unsigned char>(x + 1, y - 1);
	v[7] = img.at<unsigned char>(x + 1, y);
	v[8] = img.at<unsigned char>(x + 1, y + 1);		
		}
	int sum = v[0] + v[1] + v[2] + v[3] + v[4] + v[5] + v[6] + v[7] + v[8];
	if(sum != 0)
	{
	X = x + 1.0*(v[6] + v[7] + v[8] - v[0] - v[1] - v[2]) / sum;
	Y = y + 1.0*(v[2] + v[5] + v[8] - v[0] - v[3] - v[6]) / sum;
	}
	else
		{
		X = 0;
			Y = 0;
		}

	return getScore(x, y, img, 3);
}

float divideBack(cv::Mat image)
{
	cv::Mat bg;
	cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(30, 30));
	morphologyEx(image, bg, cv::MORPH_OPEN, element);
	cv::subtract(image, bg, image);
	float m = cv::mean(image)[0];
	/*cv::Mat b_m, b_st;
	cv::meanStdDev(image, b_m, b_st);
	std::cout << " std: " << b_st.at<double>(0, 0) << " mean : " << b_m.at<double>(0, 0) << std::endl;*/
	return m;
	//std::cout << ;
	//cv::Mat tempOut;
	//image.convertTo(tempOut, CV_16U);
	//cv::imwrite("a.tif", image);
}

int ImageFocus::AreaScore()
{
	//m_peak.clear();
	m_score.clear();
	m_Areas.clear();
	m_curArea = 0;
	m_curScore = 0;
	int c = m_Image512.cols;
	int r = m_Image512.rows;

	cv::Mat tempImg = m_Image512.clone();
	float mInts = divideBack(tempImg);
	int count = 0;
	int p12 = mInts * 2; //2.3  2
	int p11 = mInts * 1.4; //2 1.4
	int p20 = mInts * 4;

	for (int x = 3; x < r - 3; x++)
	{
		for (int y = 3; y < c - 3; y++)
		{
			unsigned short Ints = tempImg.at<unsigned char>(x, y);
			if (QImgType::mImgBits == BITS_16)
			{
				Ints = tempImg.at<unsigned short>(x, y);
			}
			bool peakFlag = false;
			if (Ints > p12) //1.4
			{
				if (Ints > p20)
					peakFlag = basicPeak(x, y, tempImg, p11);
				else
					peakFlag = goodPeakJudge(x, y, tempImg, p11);

				if (peakFlag) //find peak 1.1
				{
					int area = getXmultiY(x, y, tempImg, p11 * 0.8); //1 Img??
					if (area) //ret  trues
					{
						count++;
						cv::Point2f posi;
						float scoreVal = scoreFunc(x, y, m_Image512, posi.y, posi.x); //score Img
						//m_peak.push_back(posi);
						m_score.push_back(scoreVal);
						m_Areas.push_back(area);

					}
				}

			}
		}
	}
	if (count > 9) //old = 30
	{
		std::vector< float >  tmpVector(m_score.begin(), m_score.end());
		std::vector< float >::iterator begin = tmpVector.begin();
		std::vector< float >::iterator end = tmpVector.end();
		std::vector< float >::iterator p50 = tmpVector.begin() + count * 90 / 100; //p90
		std::nth_element(begin, p50, end);
		m_curScore = *p50;

		begin = m_Areas.begin();
		end = m_Areas.end();
		p50 = begin + count * 70 / 100;
		std::nth_element(begin, p50, end);
		m_curArea = *p50;
		//return scoreMid; // value/sqrt(meanInts) * 200
	}
	return count;
}

int ImageFocus::getBestImageIndex(int &index, float &Score, int& imgCount, int mode)
{
	int ret = getGoodImageIndexInfo(index, Score, imgCount);
	if (ret > 0)
	{
		if (ret == 4 || Score < 2.4)//not good enough : suggest check another direction
			ret += 100;
	}
	return ret;
}
int ImageFocus::getGoodImageIndexInfo(int &index, float &Score, int& imgCount)
{
	if (All_bestIndex < 0)
	{
		index = -1;
		Score = 0;
		return -1;
	}

	imgCount = imageCount;
	if (All_bestIndex > 1 && All_bestIndex < imgCount - 1) 
	{
		int scoreImgCount = 0;
		if (pub_score[All_bestIndex - 1]>0.8 || pub_score[All_bestIndex + 1] > 0.8)
		{
			for (int i = 0; i<imgCount; i++) //check Score=0
			{
				if (pub_score[i]>0.8)
					scoreImgCount++;
			}
			if (scoreImgCount < 5)
			{
				index = All_bestIndex;// -1;
				Score = All_maxScore;// 0;
				return -6;
			}
		}
	}
	if (All_maxScore>3 && All_minArea<validArea && All_bestIndex>1)//good Image
	{		
		index = All_bestIndex;
		Score = All_maxScore;		
		return 1;
	}
	else if (All_maxScore < 0.8)//all bad Image
	{
		index = -1;
		Score = 0;
		return -1;
	}

	if (All_bestIndex != bestDevIndex)
	{
		if (mDevScore>2.8 && abs(All_bestIndex - bestDevIndex) < 5 && abs(mDevScore - All_maxScore)<0.4)
		{
			//if (abs(stdDevArr[All_bestIndex] - stdDevArr[bestDevIndex]) / (stdDevArr[All_bestIndex] + stdDevArr[bestDevIndex])<0.05)

			index = (All_bestIndex + bestDevIndex) / 2;
			Score = pub_score[index];
			if (index>2 && index<imageCount - 1 && Score>2.7 && abs(All_maxScore - Score) < 0.4)							
				return 2;
			else
			{
				index = All_bestIndex;
				Score = All_maxScore;
				if (pub_Areas[index]<validArea && Score>2.8 && index >2 && index<imageCount - 1 && imageCount>5)
					return 3;
				else
					return -2;
			}

		}
		//if ((pub_stdDevArr[bestDevIndex] - pub_stdDevArr[All_bestIndex]) / (pub_stdDevArr[bestDevIndex] + pub_stdDevArr[All_bestIndex])>0.2)
		
		int ScoreErr = 0, DevErr = 0;
		if (All_bestIndex > bestDevIndex)
		{
			for (int i = bestDevIndex; i < All_bestIndex; i++)//Score > Dev
			{
				if (pub_score[i] > pub_score[i + 1])
					ScoreErr++;
				if (pub_stdDevArr[i]<pub_stdDevArr[i + 1])
					DevErr++;
			}
		}
		else
		{
			for (int i = All_bestIndex; i < bestDevIndex; i++)
			{
				if (pub_score[i] < pub_score[i + 1])
					ScoreErr++;
				if (pub_stdDevArr[i]>pub_stdDevArr[i + 1])
					DevErr++;
			}
		}
		if (DevErr>1 && ScoreErr > 1)
			return -7;
		if (DevErr<ScoreErr)
		{
			index = bestDevIndex;
			Score = mDevScore;
		}
		else
		{
			index = All_bestIndex;
			Score = All_maxScore;
		}
		if (pub_Areas[index]<validArea && index<imageCount - 1 && index >2 && imageCount > 5)//Area is wrong or Score is too low
			return 4;
		else
			return -3;
	}
	else
	{
		if (All_maxScore == 0)
		{
			double dev = 0;
			for (int i = 1; i < imageCount; i++) //i=0 is to check best Image
			{
				//int NO = i + 500;
				if (dev < pub_stdDevArr[i])
				{
					dev = pub_stdDevArr[i];
					index = i;
					Score = 0;
				}
			}
			return -4;
		}
		else
		{
			index = All_bestIndex;
			Score = All_maxScore;
			if (All_bestIndex > 1 && All_bestIndex < imageCount - 1)
				return 5;
			else
				return -5;
		}
	}
}
int ImageFocus::getCurPara(float &Score, float &Area, double &Dev)
{
	Score = m_curScore;
	Area = m_curArea;
	Dev = m_stdDevImg;
	return imageCount;
}

void ImageFocus::c4MidScore(int& peakSize, double& instensity)
{
	m_curArea = 25; //not calc	
	double midScore = 0;
	instensity = 0.0;
	//peakSize = ImgEvaluate::midScore512(m_Image512, midScore, instensity, m_Image512.rows);// 256);
	peakSize = ImageQuaEv::midScoreSelf(m_Image512, midScore, instensity);
	m_curScore = static_cast<float>(midScore);
}

int ImageFocus::ImageSent(const cv::Mat &Image, int index, float Zvalue, float &Score, int& peakSize, double& instenty)
{
	Score = 0;
	if (index == 1)
	{
		InitNewFocus();
	}
	else if (index == 0)
	{
		///check image;
		return -1;
	}

	int c = Image.cols;
	int r = Image.rows;

	//if (c >= CalcAreaWidth && r >= CalcAreaWidth)
	if (c >= CalcAreaWidth && r >= CalcAreaHeight)
	{
		//int nCenter_r = r / 2;
		//int nCenter_c = c / 2;
		//int nWidth = CalcAreaWidth;
		//int nHeight = CalcAreaHeight;

		//int row = nCenter_r - nHeight / 2 + 1;
		////cv::Rect cropRect(nCenter_c - nWidth / 2 + 1, row/*nCenter_r - nHeight / 2 + 1*/, nWidth, nHeight);
		//cv::Rect cropRect(nCenter_c - nWidth / 2 + 1, row/*nCenter_r - nHeight / 2 + 1*/, nWidth, nHeight);

		cv::Rect cropRect((c - CalcAreaWidth) / 2, (r - CalcAreaHeight) / 2, CalcAreaWidth, CalcAreaHeight);
		m_Image512 = Image(cropRect).clone();
	}
	else if (c >= 128 && r >= 128)
	{
		m_Image512 = Image.clone();
	}
	else
	{
		return -1;//图像太小了
	}

	/////////////////////
	imageCount++;
	cv::meanStdDev(m_Image512, b_m, b_st);
	m_stdDevImg = b_st.at<double>(0, 0);


	////////////////check index/////////////		
	//int pCount = AreaScore();
	c4MidScore(peakSize, instenty);
	if (maxDevScore < m_stdDevImg)
	{
		maxDevScore = m_stdDevImg;
		mDevScore = m_curScore;
		bestDevIndex = index;
	}

	if (m_curArea<validArea && All_maxScore < m_curScore) //most situation is true
	{
		All_maxScore = m_curScore;
		All_minArea = m_curArea;
		All_bestIndex = index;
		m_ScoreStop = 0;
	}
	else if (m_curScore - m_preScore > 0.5)
		m_ScoreStop = 0;
	else if (m_preScore >= m_curScore)
		m_ScoreStop++;
	m_preScore = m_curScore;
	Score = m_curScore;
	///////////
	int curIndex = imageCount;
	predifStdDev = difStdDev;
	difStdDev = 100 * (m_stdDevImg - preStdDev) / (m_stdDevImg + preStdDev);
	preStdDev = m_stdDevImg;
	pub_stdDevArr[curIndex] = m_stdDevImg;
	pub_Areas[curIndex] = m_curArea;
	pub_score[curIndex] = m_curScore;
	pub_Z[curIndex] = Zvalue;	
	
	if (index == All_bestIndex) //find a best- then step 1
		return 1;

	if (difStdDev > 2)
	{
		if (predifStdDev>1)
			wrongDirection = 0;
		else if (predifStdDev < -1)
		{
			if (++errorFlag > 2 && imageCount>10)
				return 0;
		}

		if (m_curArea>0)
		{

			if (m_curArea < validArea || m_curScore>goodScore)
				return 1;
			else
				return 2;
		}
		else  //bad image	use pCount???
			return 2;
	}
	else if (difStdDev < -2)//stop here
	{
		//wrongDirection++;
		if (wrongDirection++>4 && predifStdDev < -1 && m_ScoreStop>2)//get ScoreImage
		{
			return 0;
		}
		if (m_curArea>0)
		{
			if (m_curArea < validArea || m_curScore>goodScore)
				return 1;
			else
				return 2;
		}
		else  //bad image	use pCount???
			return 2;

	}
	else
	{
		if (All_maxScore - m_curScore > goodScore && m_ScoreStop > 3 && imageCount>8)
			return 0;

		if (m_curArea>0)
		{
			//if (maxScore < m_curScore) //most situation is true
			//{
			//	maxScore = m_curScore;
			//	minArea = m_curArea;
			//	bestIndex = index;
			//}
			//else if (maxScore - m_curScore > 1.0 && m_ScoreStop > 3)
			//	return 0;
			if (m_curArea < validArea || m_curScore>goodScore)
				return 1;
			else
				return 2;
		}
		else  //bad image
		{
			if (m_ScoreStop > 22) //maxnumber of bad image = 14
				return 0;
			else
				return 2;
		}
		
	}

}

void ImageFocus::InitNewFocus()
{
	pub_stdDevArr.clear();
	pub_score.clear();
	pub_Areas.clear();
	pub_Z.clear();
	//ImgInfoArr.clear();
	pub_stdDevArr.resize(3000, 0);
	pub_score.resize(3000, 0);
	pub_Areas.resize(3000, 0);
	pub_Z.resize(3000, 0);
	//ImgInfoArr.resize(3000);

	bestDevIndex = -1;
	maxDevScore = 0;
	mDevScore = 0;

	//stdDevFlag = true; //depend on mean
	predifStdDev = 0;
	difStdDev = 0;
	imageCount = 0;
	m_MeanImg = 0;
	wrongDirection = 0;
	errorFlag = 0;
	preStdDev = 0;
	//////////to test////////
	All_maxScore = 0;
	All_minArea = 0;
	All_bestIndex = -1;

	m_ScoreStop = 0;
}

////////////////////All image to find focal plane/////////////
template<typename T>
inline T getMidValue(const std::vector<T> &valueVec)
{
	size_t count = valueVec.size();
	if (count == 0)
	{
		return (T)0;
	}
	std::vector<T>  tmpVector(valueVec.begin(), valueVec.end());
	auto begin = tmpVector.begin();
	auto end = tmpVector.end();
	auto p50 = tmpVector.begin() + count * 80 / 100; //p90
	std::nth_element(begin, p50, end);
	return *p50;
}



inline double DevImg(const cv::Mat &img)
{
	int C = img.cols;
	int R = img.rows;
	cv::Mat b_m, b_st;
	const int Isize = 32;
	std::vector<double> VecDev;
	VecDev.reserve(1000);
	for (int r = 0; r < R - Isize; r += Isize)
	{
		for (int c = 0; c < C - Isize; c += Isize)
		{
			cv::Rect cutImgSize(r, c, Isize, Isize);
			cv::Mat cutImg = img(cutImgSize);

			cv::meanStdDev(cutImg, b_m, b_st);
			double stdDevImg = b_st.at<double>(0, 0);
			double meanImg = b_m.at<double>(0, 0);
			VecDev.push_back(stdDevImg);
		}
	}
	return getMidValue(VecDev);
}
double ImgDevDif(const cv::Mat &img)
{
	int r = img.cols;
	int c = img.rows;
	int blockSize = 248;
	int centerC = c / 2 - blockSize / 2;
	int centerR = r / 2 - blockSize / 2;
	cv::Rect cut = cv::Rect(centerR, centerC, blockSize, blockSize); //center
	return DevImg(img(cut));
}

int ImageFocus::FocalImage(const cv::Mat &Image, int index, float Zvalue, double &Dif)//step = 5.0 and 2.0
{
	Dif = 0;
	//if (index == 1)
	//	InitFocal();
	//else
	if (index < 1)
	{
		///check image;
		return -1;
	}
	int c = Image.cols;
	int r = Image.rows;

	if (c < 256 || r < 256) //img too small
		return -2;

	mFocalImgCount++;

	double Vimg = ImgDevDif(Image);
	//mVecDif[mFocalImgCount] = Vimg - mVimgPre; //1.0*(Vimg - mVimgPre) / (Vimg + mVimgPre);
	mVecDev[mFocalImgCount] = Vimg; //record
	Dif = Vimg;
	//////////////new judge//////////////
	if (Vimg - mVimgPre < 0) //down
	{
		if (upIndex > 0)
		{
			if (mVecDev[upIndex]<mVimgPre) //not judge ==
				upIndex = mFocalImgCount - 1;
		}
		else
			upIndex = mFocalImgCount - 1;
	}

	if (upIndex > FOCAL_IMGNUM && mFocalImgCount - upIndex == FOCAL_IMGNUM)
	{
		//int pIndex = upIndex;
		double maxDif = mVecDev[upIndex];
		int Half = FOCAL_IMGNUM / 2;
		int countMonotonic = 0;
		for (int i = 1; i < FOCAL_IMGNUM; i++)
		{
			if (maxDif < mVecDev[upIndex - i] || maxDif < mVecDev[upIndex + i])
			{
				upIndex = 0;
				break;
			}
			if (i + Half < FOCAL_IMGNUM)
			{
				if (mVecDev[upIndex - i] < mVecDev[upIndex - i - Half] || mVecDev[upIndex + i] < mVecDev[upIndex + i + Half])
					countMonotonic++;
			}
		}
		if (countMonotonic>1)
			upIndex = 0;
		if (upIndex>0)
		{
			mVecFocal.push_back(upIndex);
			upIndex = 0;
		}
	}
	if (upIndex > 0 && mFocalImgCount - upIndex > FOCAL_IMGNUM)
		upIndex = 0;

	mVimgPre = Vimg;
	
	return 1; //continue
}
int ImageFocus::getFocalPlane(int &index1, int &index2, int mode)
{
	index1 = 0;
	index2 = 0;
	if (upIndex > FOCAL_IMGNUM) //not get enough pic
	{
		int judgeImg = mFocalImgCount - upIndex;
		if (judgeImg > FOCAL_IMGNUM)
		{
			judgeImg = FOCAL_IMGNUM;
			///////////////
			double maxDif = mVecDev[upIndex];
			int Half = FOCAL_IMGNUM / 2;
			int countMonotonic = 0;
			for (int i = 1; i < FOCAL_IMGNUM; i++)
			{
				if (maxDif < mVecDev[upIndex - i] || maxDif < mVecDev[upIndex + i])
				{
					upIndex = 0;
					break;
				}
				if (i + Half < FOCAL_IMGNUM)
				{
					if (mVecDev[upIndex - i] < mVecDev[upIndex - i - Half] || mVecDev[upIndex + i] < mVecDev[upIndex + i + Half])
						countMonotonic++;
				}
			}
			if (countMonotonic>1)
				upIndex = 0;

			if (upIndex>0)
			{
				mVecFocal.push_back(upIndex);
				upIndex = 0;
			}

		}
		else	///////////last index should test before Vdif///////////		
		{
			double maxDif = mVecDev[upIndex];
			int Half = judgeImg / 2;
			int countMonotonic = 0;
			for (int i = 1; i < judgeImg; i++)
			{
				if (maxDif < mVecDev[upIndex - i] || maxDif < mVecDev[upIndex + i])
				{
					upIndex = 0;
					break;
				}
				if (i + Half < judgeImg)
				{
					if (mVecDev[upIndex - i] < mVecDev[upIndex - i - Half] || mVecDev[upIndex + i] < mVecDev[upIndex + i + Half])
						countMonotonic++;
				}
			}
			if (countMonotonic>1)
				upIndex = 0;
			////////////for another side////////
			if (upIndex>FOCAL_IMGNUM) //upIndex maybe 0: should test
			{
				for (int i = 1; i<FOCAL_IMGNUM; i++)
				{
					if (maxDif < mVecDev[upIndex - i])
					{
						upIndex = 0;
						break;
					}
				}
			}
			if (upIndex>0)
			{
				mVecFocal.push_back(upIndex);
				upIndex = 0;
			}
		}
	}
	if (mode == 4)
	{
		//if (mVecFocal.size() == 0)
		//{
		//	return -1;
		//}
		//else if (mVecFocal.size() == 1)
		//{
		//	index1 = mVecFocal[0];
		//	return 1;
		//}
		/////////////get index1 and index2////////
		//int pNum = mVecFocal.size();
		//int tempDif = 0;
		//int bestIndex2 = 0;
		//for (int i = pNum - 1; i>0; i--)
		//{
		//	int dif = mVecFocal[i] - mVecFocal[i - 1];
		//	//int pos = mVecFocal[i];
		//	//if (mVecDev[pos])
		//	if (dif > 10 && dif < 16)
		//	{
		//		index1 = mVecFocal[i - 1];
		//		index2 = mVecFocal[i];
		//		return 3;
		//	}
		//	if (dif> tempDif)
		//	{
		//		tempDif = dif;
		//		bestIndex2 = mVecFocal[i];
		//	}
		//}
		//{
		//	index1 = mVecFocal[0];
		//	index2 = bestIndex2;
		//	return 2;
		//}		
		if (mVecDev.size() == 0)
		{
			return -2;
		}

		float maxD = mVecDev[1];
		int maxId = 1;
		for (int i = 2; i < mFocalImgCount; i++)
		{
			if (maxD < mVecDev[i])
			{
				maxD = mVecDev[i];
				maxId = i;
			}
		}
		bool ret = true;
		float minL = mVecDev[maxId], minR = mVecDev[maxId];
		for (int i = 1; i<4; i++)
		{
			int l = maxId - i;
			int r = maxId + i;
			if (l>0)
			{
				if (mVecDev[l]<mVecDev[l - 1])
				{
					ret = false;
					break;
				}
				minL = mVecDev[l];
			}
			if (r<mFocalImgCount - 1)
			{
				if (mVecDev[r]<mVecDev[r + 1])
				{
					ret = false;
					break;
				}
				minR = mVecDev[r];
			}
		}
		float minV = minL > minR ? minR : minL;
		if (ret && maxD / minV > 1.3)// 2)
		{
			index1 = maxId;
			index2 = maxId;
			return 1;
		}
		else
			return -1;
	}
	else
	{
		if (mVecFocal.size() == 0)
		{
			return -1;
		}
		else if (mVecFocal.size() == 1)
		{
			index2 = mVecFocal[0];
			return 1;
		}
		///////////get index1 and index2////////
		int pNum = mVecFocal.size();
		int tempDif = 0;
		int bestIndex2 = 0;
		for (int i = pNum - 1; i>0; i--)
		{
			int dif = mVecFocal[i] - mVecFocal[i - 1];
			if (dif > 31 && dif < 37)
			{
				index1 = mVecFocal[i - 1];
				index2 = mVecFocal[i];
				return 3;
			}
			if (dif> tempDif)
			{
				tempDif = dif;
				bestIndex2 = mVecFocal[i];
			}
		}
		{
			index2 = bestIndex2;
			return 2;
		}
	}	
	return 0;
}
void ImageFocus::InitFocal()
{
	mFocalImgCount = 0;
	mVecDif.clear();
	mVecDev.clear();
	mVecMean.clear();
	mVecDif.resize(1000, 0); //1.0 is 700 image
	mVecDev.resize(1000, 0);
	mVecMean.resize(1000, 0);
	mStageFlag = 0;
	mPlus = 0;
	mMinus = 0;
	mfocal1 = 0;
	mfocal2 = 0;
	mDifPre = 0;
	upIndex = 0;
	mVimgPre = 0;
	mVecFocal.clear();
}
